import math, numpy
import plane

def get_rotated_plane(in_plane, angle, axe):
    delta = math.radians(angle)
    sin = math.sin(delta)
    cos = math.cos(delta)
    l, m, n = axe.cos1, axe.cos2, axe.cos3
    #print l ,m ,n
    print "angel: %003d delta: %003d" % (angle, delta)
    print "sin: %0.0004f cos: %0.0004f l,m,n: %0.0004f %0.0004f %0.0004f" % (sin, cos, l, m, n)
    M = [[l**2 +(m**2 + n**2)*cos,    l*m - l*m*cos - n*sin, l*n - l*n*cos + m*sin],
         [l*m - l*m*cos + n*sin, m**2 + (l**2 + n**2)*cos,      m*n - m*n*cos - l*sin],
         [l*n - l*n*cos - m*sin, m*n - m*n*cos + l*sin,   n*2 + (l**2+m**2)*cos]]
    
    v1, v2, v3 = in_plane.cos1, in_plane.cos2, in_plane.cos3
    v_ = [M[0][0] * v1 + M[0][1] * v2 + M[0][2]*v3,
          M[1][0] * v1 + M[1][1] * v2 + M[1][2]*v3,
          M[2][0] * v1 + M[2][1] * v2 + M[2][2]*v3]
    #print M #v_[0], v_[1], v_[2]
    result = plane.plane(0, 0)
    result.define_by_cos([v_[0], v_[1], v_[2]])
    return result
    

def test():
    v = ([1, 0, 0])
    angle = 90
    delta = math.radians(angle)
    sin = math.sin(delta)
    cos = math.cos(delta)
    l, m, n = 0, 1, 0
    sqrt = 1 # math.sqrt(l**2 + m**2 +n **2)
    
    print "angel: %003d delta: %003d" % (angle, delta)
    print "sin: %0.0004f cos: %0.0004f l,m,n: %0.0004f %0.0004f %0.0004f" % (sin, cos, l, m, n)


    M = ([l**2 +(m**2 + n**2)*cos,    l*m - l*m*cos - n * sin, l*n - l*n*cos + m * sin],
         [l*m - l*m*cos + n*sin, m**2 + (l**2 + n**2)*cos,      m*n - m*n*cos - l*sin],
         [l*n - l*n*cos - m*sin, m*n - m*n*cos + l*sin,   n*2 + (l**2+m**2)*cos])

         
    v1 = ([M[0][0] * v[0] + M[0][1] * v[1] + M[0][2]*v[2],
           M[1][0] * v[0] + M[1][1] * v[1] + M[1][2]*v[2],
           M[2][0] * v[0] + M[2][1] * v[1] + M[2][2]*v[2]])


    #print #v1
    a = plane.plane(0, 0); a.define_by_cos(v[0], v[1], v[2])
    b = plane.plane(0, 0); b.define_by_cos(v1[0], v1[1], v1[2])
    angle = a.get_angle_between(b)
    c = plane.plane(0, 0)
    c.define_by_cos(0, 1, 0)
    print 'Angle between %s and %s is %003d' % (a, b, angle)
    print "rotated, by %s" % (c)
    
    
def main():
    a = plane.plane(90, 0)
    b = plane.plane(0, 90)
    c = get_rotated_plane(a, 90, b)
    angle = a.get_angle_between(c)
    #print c.cos1, c.cos2, c.cos3
    print 'Angle between %s and %s is %003d' % (a, c, angle)
    print "rotated, by %s" % (b)
    #print c

if __name__ == '__main__': 
    print ' '
    print ' '
    print ' '
    print ' '
    test()
    print '-----------'
    main()
    print ' '
    print ' '
    print ' '
    print ' '
    
